#ifndef __A_STAR__
#define __A_STAR__

#include <iostream>
#include <vector>
#include <queue>

#include "meta_search.hpp"

namespace graph_search{
    struct Node{
        int x;
        int y;
        int g;
        int h;
        int f;
        Node(int x,int y,int g,int h):x(x),y(y),g(g),h(h),f(g+h){}
        bool operator<(const Node& rhs) const{
            return f > rhs.f;
        }
    };
    class a_star:public search{
        private:
            const int INF = 1e7;
            bool isValid(int x,int y,int n){
                return x>=0 && y>=0 && x<n && y<n;
            }
            int heuristic(int x1,int y1,int x2,int y2){
                return abs(x1-x2) + abs(y1-y2);
            }
        public:
            int find(const std::vector<std::vector<int>>& matrix,Position start,Position end){
                int n = matrix.size();
                std::vector<std::vector<bool>> dist(n,std::vector<bool>(n,false));
                std::priority_queue<Node> pq;

                pq.push(Node(start.x,start.y,0,heuristic(start.x,start.y,end.x,end.y)));
                dist[start.x][start.y] = true;

                while(!pq.empty()){
                    Node current = pq.top();
                    pq.pop();

                    if(current.x == end.x && current.y == end.y){
                        return current.g;
                    }

                    for(int i=0;i<4;i++){
                        int x = current.x + dx[i];
                        int y = current.y + dy[i];

                        if(isValid(x,y,n) && !visited[x][y] && matrix[x][y] == 0){
                            visited[x][y] =true;
                            pq.push(Node(start.x,start.y,current.g + 1,heuristic(x,y,end.x,end.y)));
                        }
                    }
                }
                return -1;
            }
        
    };
}

#endif